#define PWMR 11	//控制电机R PWM控制引脚
#define PWML 5	//控制电机L PWM控制引脚
#include <Servo.h>
#define DJ 6	//控制舵机 PWM控制引脚
Servo myservo;

String inputString = "";         // a string to hold incoming data
boolean stringComplete = false;  // whether the string is complete

int Left_motor_1=8; //左电机后退
int Left_motor_2=9; //左电机前进
 
int Right_motor_1=12; // 右电机后退
int Right_motor_2=13; // 右电机前进
 
const int SensorLeft1=15;
const int SensorLeft2=16;
const int Sensor=17;
const int SensorRight1=18;//红外循线模块
const int SensorRight2=19;
 
int S1;
int S2;
int S;
int S3;
int S4;
 
float err,last_err=0;
float P,D;

 
int kp=20,kd=0;//系数需要自己调节

int a=1;
 
void setup()
{ 
  myservo.attach(6);
  //初始化电机驱动IO为输出方式
  pinMode(Left_motor_1,OUTPUT);
  pinMode(Left_motor_2,OUTPUT);
 
  pinMode(Right_motor_1,OUTPUT);
  pinMode(Right_motor_2,OUTPUT);
 
  pinMode(SensorRight1, INPUT);
  pinMode(SensorRight2, INPUT);
  pinMode(Sensor, INPUT);
  pinMode(SensorLeft1, INPUT);
  pinMode(SensorLeft2, INPUT);


  Serial.begin(9600);// 蓝牙串口配置:
  inputString.reserve(200);
  
}
 
//=======================智能小车的基本动作=========================
 

 
void brake()//电机停止，刹车
{
  digitalWrite(Right_motor_2,1);
  digitalWrite(Right_motor_1,1);
  digitalWrite(Left_motor_2,1);
  digitalWrite(Left_motor_1,1);  
}

void runf(int x,int y)//小车驱动
{

  digitalWrite(Right_motor_1,LOW);//控制右电机
  digitalWrite(Right_motor_2,HIGH);
 
  analogWrite(PWMR,x);
 
  digitalWrite(Left_motor_1,LOW);//控制左电机
  digitalWrite(Left_motor_2,HIGH);
 
  analogWrite(PWML,x);
 
  delay(y); //执行时间，可以调整
  analogWrite(PWMR,1);
  analogWrite(PWML,1);
}

void runb(int x,int y)//小车驱动
{

  digitalWrite(Right_motor_1,HIGH);//控制右电机
  digitalWrite(Right_motor_2,LOW);
 
  analogWrite(PWMR,x);
 
  digitalWrite(Left_motor_1,HIGH);//控制左电机
  digitalWrite(Left_motor_2,LOW);
 
  analogWrite(PWML,x);
 
  delay(y); //执行时间，可以调整
  analogWrite(PWMR,1);
  analogWrite(PWML,1);
}

void runl(int x,int y)//小车驱动
{

  digitalWrite(Left_motor_1,1);//控制左电机
  digitalWrite(Left_motor_2,1);

  digitalWrite(Right_motor_1,LOW);//控制右电机
  digitalWrite(Right_motor_2,HIGH);

  analogWrite(PWML,x);
 
  delay(y); //执行时间，可以调整

}

void runr(int x,int y)//小车驱动
{
  
  digitalWrite(Right_motor_1,1);//控制右电机
  digitalWrite(Right_motor_2,1);

  digitalWrite(Left_motor_1,LOW);//控制左电机
  digitalWrite(Left_motor_2,HIGH);

  analogWrite(PWMR,x);

  delay(y); //执行时间，可以调整

}
 
int pid()//PD算法
{
  int output;
  P=err;
  D=err-last_err;
  output=kp*P+kd*D; 
  last_err=err;
  return output;
}

void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    // add it to the inputString:
    inputString += inChar;
    // if the incoming character is a newline, set a flag
    // so the main loop can do something about it:
    if (inChar == '\n') {
      stringComplete = true;
    }
  }
}
 
void loop()
{

    if (stringComplete) {
    if(inputString == "00\n"){
        a=1;
         brake();
      Serial.println(a);}
    else if(inputString == "06\n"){
        a=0;
     Serial.println(a);}
    else if(inputString == "01\n"){
      runf(100,500);
      Serial.println("qj");}
    else if(inputString == "02\n"){
      runl(110,5);
      Serial.println("y");}
    else if(inputString == "03\n"){
      runr(110,5);
      Serial.println("z");}
    else if(inputString == "04\n"){
      runb(100,500);
      Serial.println("ht");}
    else if(inputString == "05\n"){
    myservo.write(10);
    delay(1800);
    myservo.write(90);
        }
    Serial.println(inputString);
    inputString = "";
    stringComplete = false;
    }

  S1 = digitalRead(SensorRight1);
  S2 = digitalRead(SensorRight2);
  S=digitalRead(Sensor);
  S3 = digitalRead(SensorLeft1);
  S4 = digitalRead(SensorLeft2);
 if(a==0){
  if(S2==1&&S3==1&&S1==1&&S4==1)//0000此时小车在终点上电机停止
  brake();
  else {
  if((S1==0&&S2==1&&S3==1&&S4==0)||(S1==0&&S2==0&&S==1&&S3==0&&S4==0)||(S1==0&&S2==0&&S==0&&S3==0&&S4==0))//0110  00100小车居中直行
  runf(110,1);
  else if(S1==0&&S2==1&&S3==0&&S4==0)//1100  1110小车中度偏右
  runl(110,5);
  else if(S1==0&&S2==0&&S3==1&&S4==0)//0011  0111小车中度偏左
  runr(110,5);
  else if(S1==1&&S2==0&&S3==0&&S4==0)//1000小车重度偏右，需要向左大幅度偏转
  runl(120,5);
  else if(S1==0&&S2==0&&S3==0&&S4==1)//0001小车重度偏左，需要向右大幅度偏转
  runr(120,5);
      } 
  }
}